#ifndef PIDCONTROLLER_H
#define PIDCONTROLLER_H
#include <chrono>
#include <complex>
#include <optional>

namespace MM {
    enum {
        EnablePlotting = true,
        DisablePlotting = false,
    };

    enum {
        EnableSetting = true,
        DisableSetting = false,
    };

    using std::optional, std::nullopt;
    using std::chrono::time_point, std::chrono::duration, std::chrono::steady_clock, std::chrono::milliseconds,
            std::chrono::duration_cast;
    using std::abs, std::clamp;
    using std::numeric_limits;
    using TimePoint = time_point<steady_clock>;
    using std::vector, std::pair, std::string;

    struct PIDInstance {
        PIDInstance() = default;

        explicit PIDInstance(const float Kp, const float Ki, const float Kd,
                             const float MaxOut = numeric_limits<float>::max(),
                             const float IntegralLimit = numeric_limits<float>::max(), const float DeadBand = 0.0f)
            : Kp(Kp), Ki(Ki), Kd(Kd), MaxOut(MaxOut), IntegralLimit(IntegralLimit), DeadBand(DeadBand) {
        };

        template<bool Plotting = false>
        float &calculate(float measure, float ref,
                         optional<TimePoint> &&tp = nullopt) noexcept;

        float Kp{};
        float Ki{};
        float Kd{};
        float MaxOut{numeric_limits<float>::max()};
        float IntegralLimit{numeric_limits<float>::max()};
        float DeadBand{};

        // improve parameter
        // float IntegralLimit{};     // 积分限幅
        // float CoefA{};             // 变速积分 For Changing Integral
        // float CoefB{};             // 变速积分 ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
        // float Output_LPF_RC{};     // 输出滤波器 RC = 1/omegac
        // float Derivative_LPF_RC{}; // 微分滤波器系数
        //-----------------------------------
        // for calculating// improve parameter
        //        // float IntegralLimit{};     // 积分限幅
        //        // float CoefA{};             // 变速积分 For Changing Integral
        //        // float CoefB{};             // 变速积分 ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
        //        // float Output_LPF_RC{};     // 输出滤波器 RC = 1/omegac
        //        // float Derivative_LPF_RC{}; // 微分滤波器系数
        //        //------------------------------
    private:
        float Measure{};
        float Last_Measure{};
        float Err{};
        float Last_Err{};
        float Last_ITerm{};

        float Pout{};
        float Iout{};
        float Dout{};
        float ITerm{};

        float Output{};
        float Last_Output{};
        float Last_Dout{};

        float Ref{};
        TimePoint last_time{steady_clock::now()};
        vector<float> plot_buffer_x{};
        vector<float> plot_buffer_y{};
        uint16_t plot_draw_counter{};

        void integral_limit() noexcept;
    };
}

#endif //PIDCONTROLLER_H
